#include "EvolutionOfMorphologyAndBehavior.h"

#include "CrossoverOperators.h"
#include "MutationOperators.h"
#include "ReproductionOperators.h"

#include "PsoSharingMethods.h"
#include "PsoVelocityClampingMethods.h"

#include "Constraint.h"

#include "MathUtil.h"
#include "PsoInertiaUpdateMethods.h"

#include <gsl/gsl_math.h>
#include <iostream>
#include <deque>

namespace jcrada {

    EvolutionOfMorphologyAndBehavior::EvolutionOfMorphologyAndBehavior(
            int population_size, int body_parts,
            int hidden_layers, int neurons_per_layer, int max_iterations,
            double min_size, double max_size,
            double min_mass, double max_mass) :
    EvolutionOfCreatures(population_size),
    _pso(new PsoModel(population_size)),
    _morphologies(NULL),
    _creatures_morphology_index(new int[population_size]),
    _body_parts(body_parts),
    _hidden_layers(hidden_layers),
    _neurons_per_layer(neurons_per_layer),
    _max_iterations(max_iterations),
    _min_size(min_size), //Nasua nasua
    _max_size(max_size), //Giraffa camelopardalis
    _min_mass(min_mass), //Nasua nasua
    _max_mass(max_mass) {
        setupCreatures();
        setupPsoModel();
    }

    EvolutionOfMorphologyAndBehavior::~EvolutionOfMorphologyAndBehavior() {
        delete _pso;
        for (int i = 0; i < getNumberOfMorphologies(); ++i) {
            delete _morphologies[i];
        }
        delete [] _morphologies;
        delete _creatures_morphology_index;
    }

    void EvolutionOfMorphologyAndBehavior::setupCreatures() {
        for (int i = 0; i < getPopulationSize(); ++i) {
            setCreature(i, new Creature(getBodyParts(), getBodyParts() - 1,
                    getHiddenLayers(), getNeuronsPerLayer()));
        }
        std::vector<TreeOfNodes*>* morphs = TreeGenerator::createAllTrees(getBodyParts());
        setNumberOfMorphologies(morphs->size());
        _morphologies = new TreeOfNodes*[morphs->size()];
        for (int i = 0; i < morphs->size(); ++i) {
            setMorphology(i, morphs->at(i));
        }
        delete morphs;
    }

    void EvolutionOfMorphologyAndBehavior::setupPsoModel() {
        getPsoModel().setEnvy(2.0);
        getPsoModel().setNostalgia(2.0);
        getPsoModel().setLowerFitnessBetter(false);
        getPsoModel().setCurrentIteration(0);

                getPsoModel().setSharingMethod(new RingSharingMethod(2));
//        getPsoModel().setSharingMethod(new StarSharingMethod());
        //                getPsoModel().setVelocityClampingMethod(new BasicVelocityClamping());
        getPsoModel().setVelocityClampingMethod(new TanhVelocityClamping());
        //        getPsoModel().setInertiaUpdateMethod(NULL);
        getPsoModel().setInertiaUpdateMethod(new LinearDecreasingInertia(0.75 * getMaxIterations()));

        int particle_size = 0;
        //Morphology 
        //[index]
        particle_size += 1;
        //BodyPart
        //[ shape | size-x | size-y | size-z | mass ]
        particle_size += getBodyParts() * 5;
        //Constraints
        //[ connection-point | parent-connection-point |
        //  cone_swing_x_limit | cone_swing_z_limit | cone_twist_limit ]

        particle_size += (getBodyParts() - 1) * 5;
        //NeuralNetwork
        particle_size += getCreature(0).getNeuralNetwork().getTotalNumberOfWeights();

        PsoParticle particle(particle_size);
        particle.setMaxPosition(99.99999);
        particle.setMaxVelocity(25.0);
        particle.setMinPosition(0);
        particle.setSpace(PsoParticle::S_CONTINUOUS);

        PsoParticle* clone = NULL;
        for (int i = 0; i < getPsoModel().getSize(); ++i) {
            clone = particle.clone();
            getPsoModel().initialize(*clone);
            getPsoModel().setParticle(i, clone);
        }
    }

    void EvolutionOfMorphologyAndBehavior::step() {
        for (int i = 0; i < getPopulationSize(); ++i) {
            getPsoModel().getParticle(i).setFitness(getCreature(i).getFitness());
        }
        getPsoModel().step();
        for (int i = 0; i < getNumberOfMorphologies(); ++i) {
            getMorphology(i).setFitness(0.0);
        }
    }

    void EvolutionOfMorphologyAndBehavior::prepareCreatures() {
        for (int i = 0; i < getPopulationSize(); ++i) {
            Creature& creature = getCreature(i);
            creature.reset();
            double* position = getPsoModel().getParticle(i).getPosition();
            int particle_index = 0;
            //Morphology
            //[index]
            int morphology_index = (int) Math::scale(0, 100, position[particle_index++],
                    0, getNumberOfMorphologies());
            setCreaturesMorphologyIndex(i, morphology_index);
            setup(creature, getMorphology(morphology_index));
            getMorphology(morphology_index).addFitness(1.0);

            //BodyPart
            //[ shape | size-x | size-y | size-z | mass ] * creature.getBodyParts();
            for (int j = 0; j < creature.getNumberOfBodyParts(); ++j) {
                creature.getBodyPart(j).setShape(getShape(position[particle_index++]));
                creature.getBodyPart(j).setSizeX(Math::scale(0, 100, position[particle_index++],
                        getMinSize(), getMaxSize()));
                creature.getBodyPart(j).setSizeY(Math::scale(0, 100, position[particle_index++],
                        getMinSize(), getMaxSize()));
                creature.getBodyPart(j).setSizeZ(Math::scale(0, 100, position[particle_index++],
                        getMinSize(), getMaxSize()));
                creature.getBodyPart(j).setMass(Math::scale(0, 100, position[particle_index++],
                        getMinMass(), getMaxMass()));
            }
            //Constraints
            //[ parent-connection-point | connection-point |
            //  cone-swing-x | cone-twist-y | cone-swing-z ] * creature.getConstraints();
            for (int j = 0; j < creature.getNumberOfConstraints(); ++j) {
                creature.getConstraint(j).setType(Constraint::CT_CONE_TWIST);
                creature.getConstraint(j).setConnectionA((int) Math::scale(0, 100, position[particle_index++],
                        0, Constraint::MAX_CONNECTION_POINTS));
                creature.getConstraint(j).setConnectionB((int) Math::scale(0, 100, position[particle_index++],
                        0, Constraint::MAX_CONNECTION_POINTS));

                creature.getConstraint(j).setConeSwingX(Math::scale(0, 100, position[particle_index++],
                        Constraint::CONE_MIN_SWING, Constraint::CONE_MAX_SWING));
                creature.getConstraint(j).setConeTwistY(Math::scale(0, 100, position[particle_index++],
                        Constraint::CONE_MIN_TWIST, Constraint::CONE_MAX_TWIST));
                creature.getConstraint(j).setConeSwingZ(Math::scale(0, 100, position[particle_index++],
                        Constraint::CONE_MIN_SWING, Constraint::CONE_MAX_SWING));
            }
            //NeuralNetwork
            //[ weight ] * creature.getNeuralNetwork().getTotalNumberOfWeights();
            NeuralNetwork& nn = creature.getNeuralNetwork();
            int layers = nn.getNumberOfLayers();
            for (int j = 1; j < layers; ++j) {
                int neurons = nn.getLayer(j).getNumberOfNeurons();
                for (int k = 0; k < neurons; ++k) {
                    int inputs = nn.getLayer(j).getNeuron(k).getNumberOfInputs();
                    for (int l = 0; l < inputs; l++) {
                        nn.getLayer(j).getNeuron(k).setInput(l,
                                Math::scale(0, 100, position[particle_index++], -1.0, 1.0));
                    }
                    nn.getLayer(j).getNeuron(k).setThreshold(
                            Math::scale(0, 100, position[particle_index++], -1.0, 1.0));
                }
            }
            creature.set();
        }
    }

    //    BodyPart::eShape EvolutionOfMorphologyAndBehavior::getShape(double value) {
    //        //[0.0, 20.0) -> BOX
    //        //[20.0, 40.0) -> SPHERE
    //        //[40.0, 60.0) -> CAPSULE
    //        //[60.0, 80.0) -> CONE
    //        //[80.0, 100.0) -> CYLINDER
    //
    //        if (value >= 0.0 && value < 20.0) {
    //            return BodyPart::S_BOX;
    //        }
    //
    //        if (value >= 20.0 && value < 40.0) {
    //            return BodyPart::S_SPHERE;
    //        }
    //
    //        if (value >= 40.0 && value < 60.0) {
    //            double p = 20.0 / 3.0; //Portion
    //            if (value >= 40.0 && value < 40.0 + p) {
    //                return BodyPart::S_CAPSULE_X;
    //            }
    //            if (value >= 40.0 + p && value < 40.0 + (2 * p)) {
    //                return BodyPart::S_CAPSULE_Y;
    //            }
    //            if (value >= 40.0 + (2 * p) && value < 60.0) {
    //                return BodyPart::S_CAPSULE_Z;
    //            }
    //        }
    //
    //        if (value >= 60.0 && value < 80.0) {
    //            double p = 20.0 / 3.0; //Portion
    //            if (value >= 60.0 && value < 60.0 + p) {
    //                return BodyPart::S_CONE_X;
    //            }
    //            if (value >= 60.0 + p && value < 60.0 + (2 * p)) {
    //                return BodyPart::S_CONE_Y;
    //            }
    //            if (value >= 60.0 + (2 * p) && value < 80.0) {
    //                return BodyPart::S_CONE_Z;
    //            }
    //        }
    //        if (value >= 80.0 && value < 100.0) {
    //            double p = 20.0 / 3.0; //Portion
    //            if (value >= 80.0 && value < 80.0 + p) {
    //                return BodyPart::S_CYLINDER_X;
    //            }
    //            if (value >= 80.0 + p && value < 80.0 + (2 * p)) {
    //                return BodyPart::S_CYLINDER_Y;
    //            }
    //            if (value >= 80.0 + (2 * p) && value < 100.0) {
    //                return BodyPart::S_CYLINDER_Z;
    //            }
    //        }
    //        BDEBUG(TO_STRING(value));
    //        assert(0);
    //    }

    BodyPart::eShape EvolutionOfMorphologyAndBehavior::getShape(double value) {
        //[0.0, 10.0) -> BOX
        //[10.0, 20.0) -> SPHERE
        //[20.0, 30.0) -> CAPSULE
        //[30.0, 40.0) -> CONE
        //[40.0, 50.0) -> CYLINDER
        //[50.0, 60.0) -> BOX
        //[60.0, 70.0) -> SPHERE
        //[70.0, 80.0) -> CAPSULE
        //[80.0, 90.0) -> CONE
        //[90.0, 100.0) -> CYLINDER

        if (value >= 0.0 && value < 20.0) {
            return BodyPart::S_BOX;
        }

        if (value >= 20.0 && value < 40.0) {
            return BodyPart::S_SPHERE;
        }

        if (value >= 40.0 && value < 60.0) {
            double p = 20.0 / 3.0; //Portion
            if (value >= 40.0 && value < 40.0 + p) {
                return BodyPart::S_CAPSULE_X;
            }
            if (value >= 40.0 + p && value < 40.0 + (2 * p)) {
                return BodyPart::S_CAPSULE_Y;
            }
            if (value >= 40.0 + (2 * p) && value < 60.0) {
                return BodyPart::S_CAPSULE_Z;
            }
        }

        if (value >= 60.0 && value < 80.0) {
            double p = 20.0 / 3.0; //Portion
            if (value >= 60.0 && value < 60.0 + p) {
                return BodyPart::S_CONE_X;
            }
            if (value >= 60.0 + p && value < 60.0 + (2 * p)) {
                return BodyPart::S_CONE_Y;
            }
            if (value >= 60.0 + (2 * p) && value < 80.0) {
                return BodyPart::S_CONE_Z;
            }
        }
        if (value >= 80.0 && value < 100.0) {
            double p = 20.0 / 3.0; //Portion
            if (value >= 80.0 && value < 80.0 + p) {
                return BodyPart::S_CYLINDER_X;
            }
            if (value >= 80.0 + p && value < 80.0 + (2 * p)) {
                return BodyPart::S_CYLINDER_Y;
            }
            if (value >= 80.0 + (2 * p) && value < 100.0) {
                return BodyPart::S_CYLINDER_Z;
            }
        }
        BDEBUG(TO_STRING(value));
        assert(0);
    }

    void EvolutionOfMorphologyAndBehavior::setup(Creature& creature, const TreeOfNodes& morphology) {
        const IdNode* parent = dynamic_cast<const IdNode*> (morphology.getRoot());
        assert(parent);
        creature.getBodyPart(parent->getId()).setParentId(Creature::B_NONE);
        std::deque<const IdNode*> deque;
        deque.push_back(parent);
        const IdNode* child = NULL;
        int constraint = 0;
        while (deque.size() > 0) {
            parent = deque.front();
            for (int i = 0; i < parent->getArity(); ++i) {
                child = dynamic_cast<IdNode*> (parent->getChild(i));
                if (child) {
                    creature.getBodyPart(child->getId()).setParentId(parent->getId());
                    creature.getConstraint(constraint).setIdBodyA(parent->getId());
                    creature.getConstraint(constraint).setIdBodyB(child->getId());
                    ++constraint;
                    deque.push_back(child);
                }
            }
            deque.pop_front();
        }
    }

}
